#!/usr/bin/env python
import roslib; roslib.load_manifest('beginner_tutorials')
import rospy
import serial
from time import sleep
from time import time
from std_msgs.msg import Int16
from sensor_msgs.msg import RegionOfInterest

pos_x_center = 320 #Constantes con la posicion del centro
pos_y_center = 240 #de la camara

x_pos = 128	#Variables globales para mantener la posicion
y_pos = 128	#de la camara
tol = 0	#Tolerancia

# Constantes de control
Kp = 0.04
Ki = 0.8
Kd = 0.09


# Errores
ex1 = ex2 = 0
ey1 = ey2 = 0
t1 = t2 = 0
ex_integ = ey_integ = 0


# Publicadores
pub1= pub2 = 0

def callback(data):
		
	global x_pos,y_pos
	global ex1,ex2,ey1,ey2
	global t1,t2
	global pub1,pub2
	global ex_integ, ey_integ 
	

	t2 = time()	
	
	#Calcula el punto medio de la posicion de la cara
	x_center = data.x_offset + (data.width/2) 
	y_center = data.y_offset + (data.height/2)
	
	#Control P	
	Px = pos_x_center - x_center	
	Py = pos_y_center - y_center

	#Control I
	ex_integ += Px
	ey_integ += Py
	
	#Control D
	ex2 = Px
	ey2 = Py		
	Dx = (ex2 - ex1)/(t2-t1)
	Dy = (ey2 - ey1)/(t2-t1)

	#Integracion de control
	x_pos = 128 - int(Kp*(Px + Kd*Dx + Ki*ex_integ))
	y_pos = 128 + int(Kp*(Py + Kd*Dy + Ki*ey_integ))
	
	#Corrige excesos en el movimiento	
	if (x_pos > 255): x_pos = 255	
	if (x_pos < 0): x_pos = 0
	if (y_pos > 255): y_pos = 255
	if (y_pos < 0): y_pos = 0
	
	pub1.publish(pos_x_center)
	pub2.publish(x_center)

	t1 = t2
	ex1 = ex2
	ey1 = ey2	

	#Envia por serial
	s.write("\xFF\x0B")
	s.write(chr(x_pos))
	s.write("\xFF\x0A")
	s.write(chr(y_pos))

	
	

def listener():	#Listener tipico de ROS
	global t1
	global pub1,pub2	

	t1 = time()	
		
	rospy.init_node('listener', anonymous=True)
	pub1 = rospy.Publisher('entrada', Int16)
	pub2 = rospy.Publisher('salida', Int16)

	rospy.Subscriber("roi", RegionOfInterest, callback)
	rospy.spin()

if __name__ == '__main__':
	s = serial.Serial() #Configuracion Serial
	s.port = "/dev/ttyS0"
	s.baudrate = 9600
	s.open()
	if s.isOpen(): 	
		print "serial open"	
	listener()
